home *** CD-ROM | disk | FTP | other *** search
/ MacPeople 2001 February 15 / MACPEOPLE-2001-02-15.ISO.7z / MACPEOPLE-2001-02-15.ISO / オンラインウエア / 厳選オンラインウエア100 / グラフィック / 六角大王55.sit / Roku55 Folder / 技術解説プログラム < prev    next >
Text File  |  1993-11-14  |  10KB  |  419 lines
  1. /**********************************************************************
  2.      　　面対称物体の２次元座標から３次元座標を求めるプログラム
  3.  
  4.                  Copyright (C) 1993 Shusaku Furushima.
  5.  
  6. 注意：このプログラムはこのままでは動作しません。内容を理解した上で
  7. 　　　各関数／オブジェクトをご自分のプログラムで使うようにしてください。
  8.  
  9. 　このソースリストの一部または全部をあなたのプログラム（営利目的の物を含む）で
  10. 利用することを許可します。
  11. **********************************************************************/
  12.  
  13. #include <stdio.h>
  14. #include <math.h>
  15.  
  16. #define fudou double
  17.     /* max point of qr bunkai */
  18. #define RQRMP 10
  19. #define RQRNP 30
  20.  
  21. /******** Rcoord ********/
  22. typedef struct {
  23.     fudou x;
  24.     fudou y;
  25.     fudou z;
  26. } Rcoord;
  27.  
  28. /******** 4 x 4 matrix **********/
  29. typedef fudou Rmat[4][4];
  30.  
  31. /********** mathematical functions ********/
  32. extern void Rqrbunkai(int m,int n,fudou qa[][RQRNP],
  33.                     fudou qy[],fudou qz[]);
  34.  
  35. /******** general 4x4 matrix *********/
  36. class Rmat44{
  37.     void initmat(Rmat amat);
  38.   public:
  39.     void matmul(Rmat mat2);
  40.     Rmat mat;
  41.     void IRmat44();
  42.     void rotp(fudou p);
  43.     void rotq(fudou p);
  44.     void rotr(fudou p);
  45.     void copyfrom(Rmat mat2);
  46.     void move(fudou x, fudou y, fudou z);
  47.     void bepers();
  48.     void mul(fudou x, fudou y, fudou z);
  49.     void getcoord2(Rcoord *coord, Rcoord *cd, int pers);
  50. };
  51.  
  52. /********* 4x4 matrix for display objects *********/
  53. class Rmatobj{
  54.     public:
  55.     Rmat44 *devmat;            /* matrix of [3D -> 2D screen] */
  56.     Rmat44 *symm;            /* symmetrical matrix of devmat */
  57.     fudou pp, pq, pr;        /* rotate angle */
  58.     fudou dx, dy, dz;        /* move coordinate */
  59.     fudou bx, by;            /* scale to screen 2d coord */
  60.     int    pers;                /* flag if pers:1 or ortho:0 */
  61.  
  62.     void IRmatobj();                    /* init matrixes */
  63.     void makedev();                        /* make devmat */
  64.     void setsymmatx();                    /* set symm matrix of devmat */
  65.     Rcoord getdev(Rcoord *pt);            /* get screen coord */
  66.     Rcoord calc3d(Rcoord *pt1, Rcoord *pt2); /* get 3D coord from 2D coord */
  67. };
  68.  
  69.  
  70.  
  71. /********** least square method by QR (without pivoting) ***********/
  72. /* 「最小自乗法」（条件が冗長である連立方程式の解法）を解く関数 */
  73. /********* qrbunkai simple version ********/
  74. /*   m    number of unknown param.        */
  75. /*   n    number of data sets.            */
  76. /*   qa   data param matrix.              */
  77. /*   qy   data value vector.              */
  78. /*   qz   returning answer.               */
  79. /*  qrnp  define value of points number.  */
  80. /*  qrmp  define value of param number.   */
  81. /******************************************/
  82. void Rqrbunkai(int m,int n,fudou qa[][RQRNP],
  83.                 fudou qy[],fudou qz[])
  84. {
  85.  int i,j,k;
  86.  static fudou a,qq[RQRMP][RQRNP],rq[RQRMP][RQRMP];
  87.  
  88.  for( k = 1; k <= m; k++)
  89.     {
  90.     a = 0;
  91.       for(i = 1; i <=n; i++)
  92.         a = a + qa[k][i] * qa[k][i];
  93.       rq[k][k] = sqrt(a);
  94.       for( i = 1; i <= n; i++)
  95.         qq[k][i] = qa[k][i] / rq[k][k];
  96.       for( j = k+1; j <= m; j++)
  97.         {
  98.         a = 0;
  99.           for( i = 1; i <= n; i++)
  100.             a = a + qq[k][i] * qa[j][i];
  101.         rq[j][k] = a;
  102.           for( i = 1; i <= n; i++)
  103.             qa[j][i] = qa[j][i] - qq[k][i] * rq[j][k];
  104.         }
  105.     }
  106.  
  107.   for( i = 1; i <= m; i++)
  108.     {
  109.     a = 0;
  110.       for( j = 1; j <= n; j++)
  111.         a = a + qq[i][j] * qy[j];
  112.     qz[i] = a;
  113.     }
  114.   qz[m] = qz[m] / rq[m][m];
  115.   for( i = m - 1; i >= 1; i--)
  116.     {
  117.       for( j = i + 1; j <= m; j++)
  118.         qz[i] = qz[i] - qz[j] * rq[j][i];
  119.     qz[i] = qz[i] / rq[i][i];
  120.     }
  121. }
  122.  
  123. /******** mathod for general matrix Rmat44 *********/
  124. /* ４X４行列を扱う一般的な method 群　*/
  125. /*** initialise matrix ***/
  126. void Rmat44::IRmat44()
  127. {
  128.     initmat(mat);
  129. }
  130.  
  131. /*** set [I] matrix ***/
  132. void Rmat44::initmat(Rmat amat)
  133. {
  134.     int i, j;
  135.     for (i = 0; i < 4; i++){
  136.         for (j = 0; j < 4; j++){
  137.             amat[i][j] = 0.0;
  138.             if (i == j)
  139.                 amat[i][j] = 1.0;
  140.         }
  141.     }
  142. }
  143.  
  144. /*** rotate around Y axis ***/
  145. void Rmat44::rotp(fudou p)
  146. {
  147.     Rmat tmp;
  148.     
  149.     initmat(tmp);
  150.     tmp[0][0] = cos(p);
  151.     tmp[2][0] = sin(p);
  152.     tmp[0][2] = - sin(p);
  153.     tmp[2][2] = cos(p);
  154.     matmul(tmp);
  155. }
  156.  
  157. /*** rotate around X axis ***/
  158. void Rmat44::rotq(fudou p)
  159. {
  160.     Rmat tmp;
  161.     
  162.     initmat(tmp);
  163.     tmp[1][1] = cos(p);
  164.     tmp[2][1] = -sin(p);
  165.     tmp[1][2] = sin(p);
  166.     tmp[2][2] = cos(p);
  167.     matmul(tmp);
  168. }
  169.  
  170. /*** rotate around Z axis ***/
  171. void Rmat44::rotr(fudou p)
  172. {
  173.     Rmat tmp;
  174.     
  175.     initmat(tmp);
  176.     tmp[0][0] = cos(p);
  177.     tmp[1][0] = -sin(p);
  178.     tmp[0][1] = sin(p);
  179.     tmp[1][1] = cos(p);
  180.     matmul(tmp);
  181. }
  182.  
  183. /*** shift coord ***/
  184. void Rmat44::move(fudou x, fudou y, fudou z)
  185. {
  186.     Rmat tmp;
  187.     
  188.     initmat(tmp);
  189.     tmp[0][3] = x;
  190.     tmp[1][3] = y;
  191.     tmp[2][3] = z;
  192.     matmul(tmp);
  193. }
  194.  
  195. /*** set to perspective matrix ***/
  196. void Rmat44::bepers()
  197. {
  198.     Rmat tmp;
  199.     
  200.     initmat(tmp);
  201.         /* project plane is always z=1 and eye point is x=y=z=0 */
  202.     tmp[3][2] = 1.0;
  203.     matmul(tmp);
  204. }
  205.  
  206. /*** magnify value ***/
  207. void Rmat44::mul(fudou x, fudou y, fudou z)
  208. {
  209.     Rmat tmp;
  210.     
  211.     initmat(tmp);
  212.     tmp[0][0] = x;
  213.     tmp[1][1] = y;
  214.     tmp[2][2] = z;
  215.     matmul(tmp);
  216. }
  217.  
  218. /*** multiple two matrices ***/
  219. void Rmat44::matmul(Rmat mat2)
  220. {
  221.     int i, j, k;
  222.     Rmat tmp;
  223.     
  224.     for (i = 0; i < 4; i++){
  225.         for (j = 0; j < 4; j++){
  226.             tmp[i][j] = mat[i][j];
  227.         }
  228.     }
  229.     for (i = 0; i < 4; i++){
  230.         for (j = 0; j < 4; j++){
  231.             mat[i][j] = 0.0;
  232.             for (k = 0; k < 4; k++){
  233.                 mat[i][j] += tmp[k][j] * mat2[i][k];
  234.             }
  235.         }
  236.     }
  237. }
  238.  
  239. /*** copy matrix ***/
  240. void Rmat44::copyfrom(Rmat mat2)
  241. {
  242.     int i,j;
  243.     for (i = 0; i < 4; i++){
  244.         for (j = 0; j < 4; j++){
  245.             mat[i][j] = mat2[i][j];
  246.         }
  247.     }
  248. }
  249.  
  250. /*** get 2D coord from 3D coord from the matrix ***/
  251. void Rmat44::getcoord2(Rcoord *coord, Rcoord *cd, int pers)
  252. {
  253.     int i,j;
  254.     fudou w;
  255.     
  256.     coord->x = mat[0][0] * cd->x + mat[0][1] * cd->y +
  257.                 mat[0][2] * cd->z + mat[0][3];
  258.     coord->y = mat[1][0] * cd->x + mat[1][1] * cd->y +
  259.                 mat[1][2] * cd->z + mat[1][3];
  260.     coord->z = 0.0;
  261.     if (pers){
  262.         w     = mat[3][0] * cd->x + mat[3][1] * cd->y +
  263.                 mat[3][2] * cd->z + mat[3][3];
  264.         w = 1.0 / w;
  265.         coord->x *= w;
  266.         coord->y *= w;
  267.     }
  268. }
  269.  
  270. /************* method for Roku matrix Rmatobj **************/
  271. /* 六角大王用の行列操作 method 群　*/
  272. /*** initialise matrices ***/
  273. void Rmatobj::IRmatobj()
  274. {
  275.     devmat = new(Rmat44);
  276.     symm = new(Rmat44);
  277.     devmat->IRmat44();
  278.     symm->IRmat44();
  279.                         /*** You can change these values in your application ***/
  280.     pp = -0.9;                /* default angle */
  281.     pq = -0.25;
  282.     pr = 0.0;
  283.     
  284.     dx = 0.0;                /* default position */        
  285.     dy = 0.0;
  286.     dz = 1.5;
  287.     
  288.     bx = by = 100.0;        /* default magnify value */
  289.     
  290.     pers = 1;                /* default is perspective (not orthogonal) */
  291. }
  292.  
  293. /*** make matrix of [3D coord -> 2D screen coord] ***/
  294. void Rmatobj::makedev()
  295. {
  296.     devmat->IRmat44();
  297.     devmat->rotp(pp);            /* rotate around Y axis */
  298.     devmat->rotq(pq);            /* rotate around X axis */
  299.     devmat->rotr(pr);            /* rotate around Z axis */
  300.     devmat->move(dx, dy, dz);    /* shift position */
  301.     if (pers)
  302.         devmat->bepers();        /* set to perspective matrix */
  303.     dsmat->mul(bx, by, 1.0);     /* magnify to be screen scale */
  304. }
  305.  
  306. /*** get 2D screen coord from 3D coord ***/
  307. Rcoord Rmatobj::getdev(Rcoord *pt)
  308. {
  309.     Rcoord tmp;
  310.  
  311.     devmat->getcoord2    (&tmp, pt, pers);
  312.     return(tmp);
  313. }
  314.  
  315. /*** set plane symmetry matrix ***/
  316. void Rmatobj::setsymmatx()
  317. {
  318.     symm->IRmat44();
  319.     symm->mat[0][0] = - 1.0;
  320.     symm->matmul(devmat->mat);
  321. }
  322.  
  323. /*** calc 3D coord from two 2D coord ***/
  324. Rcoord Rmatobj::calc3d(Rcoord *pt1, Rcoord *pt2)
  325. {
  326.     Rcoord p;
  327.     fudou qa[RQRMP][RQRNP], qy[RQRNP], qz[RQRMP];
  328.     
  329.     qa[1][1] = devmat->mat[0][0] - devmat->mat[3][0] * pt1->x;
  330.     qa[2][1] = devmat->mat[0][1] - devmat->mat[3][1] * pt1->x;
  331.     qa[3][1] = devmat->mat[0][2] - devmat->mat[3][2] * pt1->x;
  332.     qy[1] = - devmat->mat[0][3] + devmat->mat[3][3] * pt1->x;
  333.  
  334.     qa[1][2] = devmat->mat[1][0] - devmat->mat[3][0] * pt1->y;
  335.     qa[2][2] = devmat->mat[1][1] - devmat->mat[3][1] * pt1->y;
  336.     qa[3][2] = devmat->mat[1][2] - devmat->mat[3][2] * pt1->y;
  337.     qy[2] = - devmat->mat[1][3] + devmat->mat[3][3] * pt1->y;
  338.  
  339.     qa[1][3] = symm->mat[0][0] - symm->mat[3][0] * pt2->x;
  340.     qa[2][3] = symm->mat[0][1] - symm->mat[3][1] * pt2->x;
  341.     qa[3][3] = symm->mat[0][2] - symm->mat[3][2] * pt2->x;
  342.     qy[3] = - symm->mat[0][3] + symm->mat[3][3] * pt2->x;
  343.  
  344.     qa[1][4] = symm->mat[1][0] - symm->mat[3][0] * pt2->y;
  345.     qa[2][4] = symm->mat[1][1] - symm->mat[3][1] * pt2->y;
  346.     qa[3][4] = symm->mat[1][2] - symm->mat[3][2] * pt2->y;
  347.     qy[4] = - symm->mat[1][3] + symm->mat[3][3] * pt2->y;
  348.     
  349.     Rqrbunkai(3, 4, qa, qy, qz);
  350.     p.x = qz[1];
  351.     p.y = qz[2];
  352.     p.z = qz[3];
  353.     return(p);
  354. }
  355.  
  356. /************** abstract of RokkakuDaioh 2D -> 3D routine *****************/
  357. /*** ２次元座標から３次元座標を計算する処理の流れ ***/
  358. /*　本物の六角大王ではデータはリストで持っています　*/
  359. /*  クリッピングは本物の六角大王でもやっていません！  */
  360. void main()
  361. {
  362.     Rmatobj *rmat;
  363.     
  364.     Rcoord gridscpt[MAX], gridpt[MAX];            /* グリッドのデータ */
  365.     int ngpoint, ngline, gridline[MAX][2];
  366.     
  367.     Rcoord inputscpt[MAX][2], inputpt[MAX][2];    /* 入力する線画のデータ */
  368.     int ninputpt, ninputln, inputline[MAX][2];
  369.     
  370.     short x, y;
  371.     int i;
  372.     
  373.             :
  374.     グリッドの座標と線情報を設定する。
  375.     六角大王では一辺 1.0 で、各辺が x,y,z 軸に平行な立方体を定義している。
  376.             :
  377.     rmat = new(Rmatobj);
  378.     rmat->IRmatobj();                    /* 行列の初期化 */
  379.             :
  380.     表示パラメータ（rmat->pp など）を好みに合わせて変える。
  381.             :
  382.     rmat->makedev();                    /* パラメータから透視変換行列を求める */
  383.  
  384.     for (i = 0; i < ngpoint; i++){        /* グリッドの表示座標を計算 */
  385.         gridscpt[i] = rmat->getdev(&gridpt[i]);
  386.     }
  387.     for (i = 0; i < ngline; i++){        /* グリッドをスクリーンに表示 */
  388.         x = (short)(gridscpt[ gridline[i][0] ].x);
  389.         y = (short)(gridscpt[ gridline[i][0] ].y);
  390.         MoveTo(x, y);
  391.         x = (short)(gridscpt[ gridline[i][1] ].x);
  392.         y = (short)(gridscpt[ gridline[i][1] ].y);
  393.         LineTo(x, y);
  394.     }
  395.             :
  396.     グリッドに合わせた物体の線画を入力させる。
  397.     入力された点の左右の対応づけが行われた結果、
  398.     左側の点のスクリーン上の座標が　inputscpt[i][0] に、
  399.     右側の点のスクリーン上の座標が　inputscpt[i][1] に入る。
  400.     （x,y,z のうち x,y だけに値が入っていればよい）
  401.             :
  402.     rmat->setsymmatx();                    /* 対称な行列を設定 */
  403.     for (i = 0; i < ninputpt; i++){
  404.                                         /* ３次元座標を計算 */
  405.         inputpt[i][0] = rmat->calc3d(&inputscpt[i][0], &inputscpt[i][1]);
  406.                                         /* 一方の点は X 座標の符号が逆 */
  407.         inputpt[i][1] = inputpt[i][0];
  408.         inputpt[i][1].x = - inputpt[i][1].x;
  409.     }
  410.             :
  411.     これにて、
  412.     左側の点の３次元座標が　inputpt[i][0] に、
  413.     右側の点の３次元座標が　inputpt[i][1] に入っている。
  414.     
  415. }
  416.  
  417.  
  418.  
  419.